/*
swing.c
offer dryer air port swing control motor driver
designed by lunanting

---------------------------
2020-07-27 15:16:31
create file
---------------------------
*/
#include "../dm556_driver.h"
#include "swing.h"
#include "string.h"
#ifdef OK1054W
#include "../../../board/ok1054w/board/board.h"
#else
#include "../../../board/ok1053w/board/board.h"
#endif
#include "aos/kernel.h"
#include "../pansonic/lifter.h"
#include "../../bsp/io_config.h"
#include "../leisai-485/rs57.h"

static can_node_id_t swing_node={1,1};
static int cycle_time=50;
static int top=80,mid=50,bottom=25,init_angle=5;
static int top_delay=2000,mid_delay=1500,bottom_delay=500;
static int location=0;
static int speed=0;
static int cycle_stop=1, cycle_lifter_stop=1;
static float lifter_top=0.3,lifter_bottom=0.0;
static int dir=0;
static aos_sem_t move_sem;
static bool b_swing_bottom_ignore = false;//摆动时不吹底部
static bool b_use_rs57_swing = true;//是否使用RS57驱动器的摆动机构

int32_t set_profile_velocity(const can_node_id_t *node_id, uint32_t val);
void xp_swing_cycle(void *arg);
void xp_swing_lifter_cycle(void *arg);

/*
init swing 
param:
    slave_id:set swing slave id
return:
    0=success
*/
int xp_swing_init(u8 slave_id){
    int sta=0;

    b_use_rs57_swing = false;

    aos_task_new("swing lifter",xp_swing_lifter_cycle,NULL,2100);
    s_profile_position_config config;
    if(xp_canopen_dev_manager_init()==-1){
        return -1;
    }
    swing_node.node=slave_id;
    aos_msleep(2000);
    sta=enable_dm556(&swing_node);
    if(sta!=0){
        println("DM556 driver init fail~");
        return -2;
    }
    config.motor_resolution=2000;
    config.profile_velocity=SWING_speed/60*2000;
    config.end_velocity=2000;
    config.profile_acceleration=20000;
    config.profile_deceleration=20000;
    sta=profile_position_config(&swing_node,&config);
    if(sta!=0){
        println("DM556 driver config fail~");
        return -3;
    }
    speed=config.profile_velocity;
    set_target_velocity(&swing_node,SWING_speed/60*2000);
    set_firEN_func(&swing_node,1);
    set_firTime_func(&swing_id,512);
    aos_task_new("swing cycle",xp_swing_cycle,NULL,2100);
    aos_sem_new(&move_sem,1);
    xp_swing_check_zero();
    return 0;
}



/*
swing move angle
param:
    angle:move angle
return:
    0=success
*/
int xp_swing_move(int angle){
    int sta=0;
    int pluse=0;
    int dis;
	int delay;
    u64 time_last=0;

    if(angle==location)return 0;
    if(aos_sem_wait(&move_sem,5000)!=0){
        return 1;
    }
    xp_swing_locker_set(1);
    dis=angle-location;
    pluse=dis*2000*SWING_ratio/360;//风机摆动电机单圈编码器脉冲2000cnt
    if(angle>location){
        dir=1;
        /*if(xp_io_read_input_pin(1,SWING_io_limit_top)==SWING_io_touch){
            println("swing top limit io touched~");
            sta=0;
            goto END;
        }*/
    }
    else if(angle<location){
        dir=-1;
        if(xp_io_read_input_pin(1,SWING_io_limit_bottom)==SWING_io_touch){
            xp_swing_stop();
            location=-10;//底部传感器安装在-10°
            println("swing bottom limit io touched~ location set %d", location);
            sta=0;
            goto END;
        }
    }
    sta=relative_position_move(&swing_node,pluse);
    if(sta!=0){
        sta=-1;
        goto END;
    }
    time_last=aos_now_ms();
    location=angle;
	delay=((float)abs(pluse)/speed+((float)speed/18000))*1000;
    while(1){
        if((aos_now_ms()-time_last)>delay){
            break;
        }
        if(dir==-1){
            if(xp_io_read_input_pin(1,SWING_io_limit_bottom)==SWING_io_touch){
                xp_swing_stop();
                location=-10;//底部传感器安装在-10°
                println("swing bottom limit io touched~ location set %d", location);
                break;
            }
        }
        if(dir==1){
            /*if(xp_io_read_input_pin(1,SWING_io_limit_top)==SWING_io_touch){
                xp_swing_stop();
                location=120;//顶部传感器安装在120°
                println("swing top limit io touched~");
                break;
            }*/
        }
        aos_msleep(10);
    }
    xp_swing_locker_set(0);
    dir=0;
END:aos_sem_signal(&move_sem);//释放锁
    return sta;
}

/*
swing stop
return:
    0=success
*/
int xp_swing_stop(void){
    dir=0;
    return slow_down(&swing_node,50000);
}

int xp_swing_change_speed(u32 speed)
{
	int ret = -1;
	
	if (speed > 500){
		println("swing speed %d out of range!", speed);
		return -1;
	}

	ret = set_profile_velocity(&swing_node,speed/60*2000);
	if (ret < 0){
		println("swing speed set failed! ret %d", ret);
		return -2;
	}

	return 0;
}

/*
swing cycle thread
*/
void xp_swing_cycle(void *arg){
	xp_swing_change_speed(SWING_speed);

    while(true){
        if(cycle_stop==1){
            aos_msleep(100);
            continue;
        }

		//风机摆动动作
        xp_swing_move(top);
        aos_msleep(cycle_time+top_delay);
		/* 吹车辆上部时摆动较慢，提升上部吹风效果 */
		//xp_swing_change_speed(SWING_speed/2);
		xp_swing_move(mid);
		aos_msleep(cycle_time+mid_delay);
		if(false == xp_swing_bottom_ignore_get()){
			xp_swing_move(top);
			aos_msleep(cycle_time+top_delay);
			xp_swing_move(bottom);
			aos_msleep(cycle_time+bottom_delay);
		}
		
        if(cycle_stop==1){
			xp_swing_move(init_angle);
            println("dryer air swing end~");
        }
    }
}


/*
swing lifter cycle
*/
void xp_swing_lifter_cycle(void *arg){

    while(true){
         if(cycle_lifter_stop==1){
            aos_msleep(100);
            continue;
        }
        xp_lifter_move(lifter_top,1);
        aos_msleep(500);
        xp_lifter_move(lifter_bottom,1);
        aos_msleep(500);
        if(cycle_lifter_stop==1){
            xp_lifter_move(0,1);
        }
    }
}

/*
swing motor locker
param:
    sta: locker status 0=close,1=open
*/
void xp_swing_locker_set(u8 sta){
    static u8 locker=0;

    if(locker!=sta){
        locker=sta;
        if(locker==0){
            xp_io_write_pin(1,SWING_io_locker,1);
        }
        else{
            xp_io_write_pin(1,SWING_io_locker,0);
        }
        aos_msleep(50);
    }
}

/*
limit check
*/
void xp_swing_limit_check(void){
    static u8 flag=0;

    if(dir==1){
        if(xp_io_read_input_pin(1,SWING_io_limit_top)==SWING_io_touch){
            if(flag!=1){
                flag=1;
                cycle_stop=1;
                xp_swing_stop();
                println("swing top limit io touched~");
            }
        }
        else{
            flag=0;
        }
    }
    else if(dir==-1){
        if(xp_io_read_input_pin(1,SWING_io_limit_bottom)==SWING_io_touch){
            if(flag!=2){
                flag=2;
                cycle_stop=1;
                xp_swing_stop();
                println("swing bottom limit io touched~");
            }
        }
        else{
            flag=0;
        }
    }
}


/*
check zero
return:
    0=success
*/
int xp_swing_check_zero(void){
    if(xp_swing_move(-90)!=0)return -1;
    location = 0;
    aos_msleep(500);
    if(xp_swing_move(init_angle)!=0)return -1;

    return 0;
}

/*
swing cycle set
param:
    sta:0=stop,1=start
*/
void xp_swing_cycle_set(u8 sta){
    if(sta==0){
        cycle_stop=1;
    }
    else{
        cycle_stop=0;
    }
}

/*
lifter cycle set
param:
    sta:0=stop,1=start
*/
void xp_lifter_cycle_set(u8 sta){
    if(sta==0){
        cycle_lifter_stop=1;
    }
    else  if(sta==1){
        cycle_lifter_stop=0;
    }
}


/*
set swing top delay time
param:
    delay:delay time, ms
*/
void xp_swing_topDelay_set(u32 delay){
    top_delay=delay;
}

/*
set swing angle
param:
    b:bottom angle
    t:top angle
*/
void xp_swing_angle_set(u8 b,u8 t){
    bottom=b;
    top=t;
}

/*
set swing bottom ignore flag
param:
    delay:delay time, ms
*/
void xp_swing_bottom_ignore_set(bool b_ignore){
    b_swing_bottom_ignore = b_ignore;
}

bool xp_swing_bottom_ignore_get()
{
	return b_swing_bottom_ignore;
}

/******************************rs57 swing interfaces******************************/
static int swing_id = 8;      //swing demo id
static int rs57_cycle_stop = 1;
void xp_rs57_swing_cycle(void *arg);
extern bool xp_swing_bottom_ignore_get();

/*
    用于不重启恢复时重新配置RS57风口摆动
*/
int xp_rs57_swing_recover(){
    RS57 *motor;
    int pin[8];

    motor=xp_rs57_motor_get(swing_id);
    if(motor==NULL){
        perr("rs57 swing get motor fail.");
        return -1;
    }

    pin[1]=0;
    pin[2]=1;
    pin[3]=2;
    pin[4]=3;
    pin[5]=19;
    pin[6]=21;
    pin[7]=22;

    xp_rs57_si_config(motor,1,pin);
    xp_rs57_so_config(motor,1,2,9,22);
    xp_rs57_structure_config(motor,28,1);
    // xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,1,13,0);

    if(xp_rs57_route_target_updata(motor,0,7778)!=0){   //10度
        perr("swint route 0 config fail! use default config.");
    }
    if(xp_rs57_route_target_updata(motor,1,23333)!=0){  //30度
        perr("swint route 1 config fail! use default config.");
    }
    if(xp_rs57_route_target_updata(motor,2,38888)!=0){  //50度
        perr("swint route 2 config fail! use default config.");
    }
}

/*
swing demo init
param:
    port: uart port
    speed: uart speed
return:
    0=success
*/
int xp_rs57_swing_init(int port,int speed){
    RS57 *motor;
    int pin[8];

    b_use_rs57_swing = true;

    motor=xp_rs57_new(port,speed,swing_id);
    if(motor==NULL){
        return -1;
    }
    pin[1]=0;
    pin[2]=1;
    pin[3]=2;
    pin[4]=3;
    pin[5]=19;
    pin[6]=21;
    pin[7]=22;

    // for(int i=1;i<7;i++){
    //     pin[i+1]=i;
    // }

    xp_rs57_si_config(motor,1,pin);
    xp_rs57_so_config(motor,1,2,9,22);
    xp_rs57_structure_config(motor,28,1);
    // xp_rs57_en_set(motor,1);
    xp_rs57_hold_config(motor,1,13,0);

    if(xp_rs57_route_target_updata(motor,0,7778)!=0){   //10度
        perr("swint route 0 config fail! use default config.");
    }
    if(xp_rs57_route_target_updata(motor,1,23333)!=0){  //30度
        perr("swint route 1 config fail! use default config.");
    }
    if(xp_rs57_route_target_updata(motor,2,38888)!=0){  //50度
        perr("swint route 2 config fail! use default config.");
    }
	if(0 != aos_task_new("rs57_swing cycle",xp_rs57_swing_cycle,NULL,2100)){
		printf("create rs57_swing cycle thread fail");
		return -1;
	}
}

/*
swing demo route move 
param:
    route: route number, 0=origin,1=any point,2=max point,3=check zero
return:
    0=success
*/
int xp_rs57_swing_route_set(u8 route){
    RS57 *motor=NULL;
    int ret=0;

    if(route>3){
        perr("rs57 swing demo route set fail, route =%d.",route);
        return -1;
    }
    motor=xp_rs57_motor_get(swing_id);
    if(motor==NULL){
        perr("rs57 swing demo get motor fail.");
        return -2;
    }
    ret=xp_rs57_route_set(motor,route);
    if(ret!=0){
        perr("rs57 swing demo route set fail, ret=%d.",ret);
        return -3;
    }
    return 0;
}

/*
swing demo move
param:
    angle: move to angle
return:
    0=success
*/
int xp_rs57_swing_move(int angle){
    RS57 *motor=NULL;
    int sta=0;

    if(angle<0||angle>90){
        perr("swing move angle fail. angle=%d.",angle);
        return -1;
    }
    motor=xp_rs57_motor_get(swing_id);
    if(motor==NULL){
        perr("swing motor get fail.");
        return -2;
    }
    int a=xp_rs57_angle_to_pluse(motor,angle);
    sta=xp_rs57_route_target_updata(motor,1,a);
    if(sta!=0){
        perr("swing move target update fail. sta=%d.",sta);
        return -3;
    }
    sta=xp_rs57_route_set(motor,1);
    if(sta!=0){
        perr("swing move, set route 1 fail .");
        return -4;
    }
    println("rs57 swing move to: %d degree.",angle);
    return 0;
}

int xp_rs57_swing_checkZero(void)
{
	int ret = -1;
	uint16_t wait_cnt = 0;
	RS57 *motor = xp_rs57_motor_get(swing_id);

	if(NULL == motor){
		println("xp_rs57_swing_checkZero: xp_rs57_motor_get %d failed!", swing_id);
		return -1;
	}

	xp_rs57_hold_set(motor,1);
	aos_msleep(700);//等待抱闸完全松开
	
	ret = xp_rs57_swing_route_set(3);
	if(ret != 0){
		println("xp_rs57_swing_route_set 3 failed! ret %d", ret);
		return -2;
	}
	aos_msleep(1000);

	//等待动作完成后关闭抱闸
	while(1){
		aos_msleep(500);
		if(0 == motor->status.mc_ok){
			println("xp_rs57_swing_route_set 3 complete!");
			break;
		}
		wait_cnt++;
		if(wait_cnt > 40){
			//20s未完成动作，返回错误
			return -3;
		}
	}
	xp_rs57_hold_set(motor,0);

	return 0;
}

/*
rs57_swing cycle thread
*/
void xp_rs57_swing_cycle(void *arg){
    RS57* motor = xp_rs57_motor_get(swing_id);
    if(NULL == motor){
		println("xp_rs57_motor_get %d failed! swing_cycle thread can't create!", swing_id);
		return;
	}

    while(true){
        if(rs57_cycle_stop==1){
            aos_msleep(500);
            continue;
        }

		//风机摆动动作
		xp_rs57_hold_set(motor,1);
		xp_rs57_swing_route_set(2);//50度
		aos_msleep(2500);
		xp_rs57_swing_route_set(1);//30度
		aos_msleep(1000);
		if(false == xp_swing_bottom_ignore_get()){
			xp_rs57_swing_route_set(2);//50度
			aos_msleep(2500);
			xp_rs57_swing_route_set(0);//10度
			aos_msleep(500);
		}
		
		if(rs57_cycle_stop==1){
			xp_rs57_swing_route_set(3);
			if(0 == motor->status.mc_ok){
				//动作完成后抱闸关闭
				xp_rs57_hold_set(motor,0);
			}
			println("dryer air rs57_swing end~");
		}
    }
}

/*
rs57_swing cycle set
param:
    sta:0=stop,1=start
*/
void xp_rs57_swing_cycle_set(u8 sta){
    if(sta==0){
        rs57_cycle_stop=1;
    }
    else{
        rs57_cycle_stop=0;
    }
}

/**************************************debug funciton**************************/
/*
swing debug
*/
int xp_swing_debug(char *type,char *fun,char *param){
    if(strcmp(type,"swing")!=0)return 0;
    if(strcmp(fun,"move")==0){
        int angle=atoi(param);
        if(false == b_use_rs57_swing){
            xp_swing_move(angle);
        }else{
            RS57 *motor = xp_rs57_motor_get(swing_id);
            xp_rs57_hold_set(motor, 1);
            xp_rs57_swing_move(atoi(param));
        }
    }
    else if(strcmp(fun,"stop")==0){
        cycle_stop=1;
    }
    else if(strcmp(fun,"start")==0){
        cycle_stop=0;
    }
    else if(strcmp(fun,"cycle_time")==0){
        cycle_time=atoi(param);
    }
    else if(strcmp(fun,"top")==0){
        top=atoi(param);
    }
    else if(strcmp(fun,"bottom")==0){
        bottom=atoi(param);
    }
    else if(strcmp(fun,"set_speed")==0){
        speed=atoi(param)/60*2000;
        set_profile_velocity(&swing_node,speed);
    }
    else if(strcmp(fun,"lifter_top")==0){
        lifter_top=atof(param);
    }
    else if(strcmp(fun,"lifter_bottom")==0){
        lifter_bottom=atof(param);
    }
    else if(strcmp(fun,"lifter_start")==0){
        cycle_lifter_stop=0;
    }
    else if(strcmp(fun,"lifter_stop")==0){
        cycle_lifter_stop=1;
    }
    else if(strcmp(fun,"locker")==0){
        if(strcmp(param,"close")==0){
            xp_swing_locker_set(0);
        }
        else if(strcmp(param,"open")==0){
            xp_swing_locker_set(1);
        }
    }
    else if(strcmp(fun,"check_zero")==0){
        xp_swing_check_zero();
    }
    else if(strcmp(fun,"set_top_delay")==0){
        top_delay=atoi(param);
        println("top delay set as %d ms",top_delay);
    }
    else if(strcmp(fun,"set_bottom_delay")==0){
        bottom_delay=atoi(param);
        println("bottom delay set as %d ms",bottom_delay);
    }
    else if(strcmp(fun,"route")==0){
        RS57 *motor = xp_rs57_motor_get(swing_id);
        xp_rs57_hold_set(motor, 1);
        xp_rs57_swing_route_set(atoi(param));
    }
    else{
        return 0;
    }
    return 1;
}

//end of the file
